#include "UtilityPCH.h"

#include "UT_MathHelper.h"
#include "UT_Log.h"
#include "UT_TAlgorithm.h"
#include "UT_SpectrumRGB.h"
#include "UT_SpectrumSampled.h"

using namespace Utility;

//----------------------------------------------------------------------------------
UT_Quaternions UT_MathHelper::Lerp(float t, const UT_Quaternions& q1, const UT_Quaternions& q2)
{
	float cosTheta = Dot(q1, q2);
	if (cosTheta > .9995f)
		return Normalize((1.f - t) * q1 + t * q2);
	else 
	{
		float theta			 = ACos(Clamp(cosTheta, -1.f, 1.f));
		float thetap		 = theta * t;
		UT_Quaternions qperp = Normalize(q2 - q1 * cosTheta);
		return q1 * Cos(thetap) + qperp * Sin(thetap);
	}
}
//-----------------------------------------------------------------------------------
UT_Matrix4x4Float UT_MathHelper::Transpose(const UT_Matrix4x4Float &m)
{
	return UT_Matrix4x4Float(m.m[0][0], m.m[1][0], m.m[2][0], m.m[3][0],
		m.m[0][1], m.m[1][1], m.m[2][1], m.m[3][1],
		m.m[0][2], m.m[1][2], m.m[2][2], m.m[3][2],
		m.m[0][3], m.m[1][3], m.m[2][3], m.m[3][3]);
}
//-------------------------------------------------------------------------------------------
UT_Matrix4x4Float UT_MathHelper::Inverse(const UT_Matrix4x4Float &m)
{
	int indxc[4], indxr[4];
	int ipiv[4] = { 0, 0, 0, 0 };
	float minv[4][4];
	memcpy(minv, m.m, 4*4*sizeof(float));
	for (int i = 0; i < 4; i++) {
		int irow = -1, icol = -1;
		float big = 0.;
		// Choose pivot
		for (int j = 0; j < 4; j++) {
			if (ipiv[j] != 1) {
				for (int k = 0; k < 4; k++) {
					if (ipiv[k] == 0) {
						if (UT_MathHelper::Abs(minv[j][k]) >= big) {
							big = float(UT_MathHelper::Abs(minv[j][k]));
							irow = j;
							icol = k;
						}
					}
					else if (ipiv[k] > 1)
						GfxWriteLog(LOG_D_DEFAULT_WARNING,"Singular matrix in MatrixInvert");
				}
			}
		}
		++ipiv[icol];
		// Swap rows _irow_ and _icol_ for pivot
		if (irow != icol) {
			for (int k = 0; k < 4; ++k)
				ut_swap(minv[irow][k], minv[icol][k]);
		}
		indxr[i] = irow;
		indxc[i] = icol;
		if (minv[icol][icol] == 0.)
			GfxWriteLog(LOG_D_DEFAULT_WARNING,"Singular matrix in MatrixInvert");

		// Set $m[icol][icol]$ to one by scaling row _icol_ appropriately
		float pivinv = 1.f / minv[icol][icol];
		minv[icol][icol] = 1.f;
		for (int j = 0; j < 4; j++)
			minv[icol][j] *= pivinv;

		// Subtract this row from others to zero out their columns
		for (int j = 0; j < 4; j++) {
			if (j != icol) {
				float save = minv[j][icol];
				minv[j][icol] = 0;
				for (int k = 0; k < 4; k++)
					minv[j][k] -= minv[icol][k]*save;
			}
		}
	}
	// Swap columns to reflect permutation
	for (int j = 3; j >= 0; j--) {
		if (indxr[j] != indxc[j]) {
			for (int k = 0; k < 4; k++)
				ut_swap(minv[k][indxr[j]], minv[k][indxc[j]]);
		}
	}
	return UT_Matrix4x4Float(minv);
}
//-------------------------------------------------------------------------------------------
UT_Transform UT_MathHelper::Inverse(const UT_Transform &t)
{
	return UT_Transform(t.mInv, t.m);
}
//-----------------------------------------------------------------------------------
UT_Matrix4x4Float UT_MathHelper::Mul(const UT_Matrix4x4Float &m1, const UT_Matrix4x4Float &m2)
{
	UT_Matrix4x4Float r;
	for (int i = 0; i < 4; ++i)
		for (int j = 0; j < 4; ++j)
			r.m[i][j] = m1.m[i][0] * m2.m[0][j] +
			m1.m[i][1] * m2.m[1][j] +
			m1.m[i][2] * m2.m[2][j] +
			m1.m[i][3] * m2.m[3][j];
	return r;
}
//-----------------------------------------------------------------------------------
bool UT_MathHelper::SolveLinearSystem2x2(const float A[2][2],	const float B[2], float *x0, float *x1) 
{
	float det = A[0][0]*A[1][1] - A[0][1]*A[1][0];
	if (Abs(det) < 1e-10f)
		return false;
	
	*x0 = (A[1][1]*B[0] - A[0][1]*B[1]) / det;
	*x1 = (A[0][0]*B[1] - A[1][0]*B[0]) / det;
	
	if (isnan(*x0) || isnan(*x1))
		return false;
	return true;
}
//-----------------------------------------------------------------------------------
bool UT_MathHelper::SolveQuadraticFunction(float A,float B,float C,float *x0,float *x1)
{
	// Find quadratic discriminant
	float discrim = B * B - 4.f * A * C;
	if (discrim <= 0.) return false;
	float rootDiscrim = Sqrt(discrim);

	// Compute quadratic _t_ values
	float q;
	if (B < 0) q = -.5f * (B - rootDiscrim);
	else       q = -.5f * (B + rootDiscrim);
	*x0 = q / A;
	*x1 = C / q;
	if (*x0 > *x1) ut_swap(*x0, *x1);
	return true;
}
//-----------------------------------------------------------------------------------
void UT_MathHelper::SolveWeingartenEquation(PARAM_IN const UT_Vec3Float& dpdu,PARAM_IN const UT_Vec3Float& dpdv, 
	PARAM_IN const UT_Vec3Float& d2Pduu,PARAM_IN const UT_Vec3Float& d2Pduv,PARAM_IN const UT_Vec3Float& d2Pdvv,
	PARAM_OUT UT_Normal3Float* dndu,UT_Normal3Float* dndv)
{
	float E			= UT_MathHelper::Dot(dpdu, dpdu);
	float F			= UT_MathHelper::Dot(dpdu, dpdv);
	float G			= UT_MathHelper::Dot(dpdv, dpdv);
	UT_Vec3Float N	= UT_MathHelper::Normalize(UT_MathHelper::Cross(dpdu, dpdv));
	float e			= UT_MathHelper::Dot(N, d2Pduu);
	float f			= UT_MathHelper::Dot(N, d2Pduv);
	float g			= UT_MathHelper::Dot(N, d2Pdvv);

	float invEGF2 = 1.f / (E*G - F*F);
	*dndu = UT_Normal3Float((f*F - e*G) * invEGF2 * dpdu +
		(e*F - f*E) * invEGF2 * dpdv);
	*dndv = UT_Normal3Float((g*F - f*G) * invEGF2 * dpdu +
		(f*F - g*E) * invEGF2 * dpdv);
}
//-----------------------------------------------------------------------------------
void UT_MathHelper::Decompose(const UT_Matrix4x4Float& m, UT_Vec3Float* T, UT_Quaternions* Rquat, UT_Matrix4x4Float* S)
{
	// Extract translation _T_ from transformation matrix
	T->x = m.m[0][3];
	T->y = m.m[1][3];
	T->z = m.m[2][3];

	// Compute new transformation matrix _M_ without translation
	UT_Matrix4x4Float M = m;
	for (int i = 0; i < 3; ++i)
		M.m[i][3] = M.m[3][i] = 0.f;
	M.m[3][3] = 1.f;

	// Extract rotation _R_ from transformation matrix
	float norm;
	int count = 0;
	UT_Matrix4x4Float R = M;
	do {
		// Compute next matrix _Rnext_ in series
		UT_Matrix4x4Float Rnext;
		UT_Matrix4x4Float Rit = Inverse(Transpose(R));
		for (int i = 0; i < 4; ++i)
			for (int j = 0; j < 4; ++j)
				Rnext.m[i][j] = 0.5f * (R.m[i][j] + Rit.m[i][j]);

		// Compute norm of difference between _R_ and _Rnext_
		norm = 0.f;
		for (int i = 0; i < 3; ++i) {
			float n =	Abs(R.m[i][0] - Rnext.m[i][0]) +
						Abs(R.m[i][1] - Rnext.m[i][1]) +
						Abs(R.m[i][2] - Rnext.m[i][2]);
			norm = Max(norm, n);
		}
		R = Rnext;
	} while (++count < 100 && norm > .0001f);
	// XXX TODO FIXME deal with flip...
	*Rquat = UT_Quaternions(R);

	// Compute scale _S_ using rotation and original matrix
	*S = Mul(Inverse(R), M);
}
//-----------------------------------------------------------------------------------
UT_Transform UT_MathHelper::MakeTransform_Translate(const UT_Vec3Float &delta) 
{
	UT_Matrix4x4Float m(1, 0, 0, delta.x,
						0, 1, 0, delta.y,
						0, 0, 1, delta.z,
						0, 0, 0,       1);
	UT_Matrix4x4Float minv(1, 0, 0, -delta.x,
						   0, 1, 0, -delta.y,
						   0, 0, 1, -delta.z,
						   0, 0, 0,        1);
	return UT_Transform(m, minv);
}
//-----------------------------------------------------------------------------------
UT_Transform UT_MathHelper::MakeTransform_Scale(float x, float y, float z) 
{
	UT_Matrix4x4Float m(x, 0, 0, 0,
						0, y, 0, 0,
						0, 0, z, 0,
						0, 0, 0, 1);
	UT_Matrix4x4Float minv(1.f/x,     0,     0,     0,
							0,     1.f/y,    0,     0,
							0,        0,     1.f/z, 0,
							0,        0,     0,     1);
	return UT_Transform(m, minv);
}
//-----------------------------------------------------------------------------------
UT_Transform UT_MathHelper::MakeTransform_RotateX(float angle) 
{
	float sin_t = Sin(Radians(angle));
	float cos_t = Cos(Radians(angle));
	UT_Matrix4x4Float m(1,     0,      0, 0,
						0, cos_t, -sin_t, 0,
						0, sin_t,  cos_t, 0,
						0,     0,      0, 1);
	return UT_Transform(m, UT_MathHelper::Transpose(m));
}
//-----------------------------------------------------------------------------------
UT_Transform UT_MathHelper::MakeTransform_RotateY(float angle) {
	float sin_t = Sin(Radians(angle));
	float cos_t = Cos(Radians(angle));
	UT_Matrix4x4Float m( cos_t,   0,  sin_t, 0,
							 0,   1,      0, 0,
						-sin_t,   0,  cos_t, 0,
							 0,   0,   0,    1);
	return UT_Transform(m, UT_MathHelper::Transpose(m));
}
//-----------------------------------------------------------------------------------
UT_Transform UT_MathHelper::MakeTransform_RotateZ(float angle) 
{
	float sin_t = Sin(Radians(angle));
	float cos_t = Cos(Radians(angle));
	UT_Matrix4x4Float m(cos_t, -sin_t, 0, 0,
						sin_t,  cos_t, 0, 0,
							0,      0, 1, 0,
							0,      0, 0, 1);
	return UT_Transform(m, UT_MathHelper::Transpose(m));
}
//-----------------------------------------------------------------------------------
UT_Transform UT_MathHelper::MakeTransform_Rotate(float angle, const UT_Vec3Float &axis) 
{
	UT_Vec3Float a	= Normalize(axis);
	float s			= Sin(Radians(angle));
	float c			= Cos(Radians(angle));
	float m[4][4];

	m[0][0] = a.x * a.x + (1.f - a.x * a.x) * c;
	m[0][1] = a.x * a.y * (1.f - c) - a.z * s;
	m[0][2] = a.x * a.z * (1.f - c) + a.y * s;
	m[0][3] = 0;

	m[1][0] = a.x * a.y * (1.f - c) + a.z * s;
	m[1][1] = a.y * a.y + (1.f - a.y * a.y) * c;
	m[1][2] = a.y * a.z * (1.f - c) - a.x * s;
	m[1][3] = 0;

	m[2][0] = a.x * a.z * (1.f - c) - a.y * s;
	m[2][1] = a.y * a.z * (1.f - c) + a.x * s;
	m[2][2] = a.z * a.z + (1.f - a.z * a.z) * c;
	m[2][3] = 0;

	m[3][0] = 0;
	m[3][1] = 0;
	m[3][2] = 0;
	m[3][3] = 1;

	UT_Matrix4x4Float mat(m);
	return UT_Transform(mat, UT_MathHelper::Transpose(mat));
}
//-----------------------------------------------------------------------------------
UT_Transform UT_MathHelper::MakeTransform_LookAt(const UT_Point3Float &pos, const UT_Point3Float &look, const UT_Vec3Float &up)
{
	float m[4][4];
	// Initialize fourth column of viewing matrix
	m[0][3] = pos.x;
	m[1][3] = pos.y;
	m[2][3] = pos.z;
	m[3][3] = 1;

	// Initialize first three columns of viewing matrix
	UT_Vec3Float dir	= Normalize(look - pos);
	UT_Vec3Float left	= Normalize(Cross(Normalize(up), dir));
	UT_Vec3Float newUp	= Cross(dir, left);
	m[0][0] = left.x;
	m[1][0] = left.y;
	m[2][0] = left.z;
	m[3][0] = 0.;
	m[0][1] = newUp.x;
	m[1][1] = newUp.y;
	m[2][1] = newUp.z;
	m[3][1] = 0.;
	m[0][2] = dir.x;
	m[1][2] = dir.y;
	m[2][2] = dir.z;
	m[3][2] = 0.;
	UT_Matrix4x4Float camToWorld(m);
	return UT_Transform(UT_MathHelper::Inverse(camToWorld), camToWorld);
}
//-----------------------------------------------------------------------------------
/**
*	MakeTransform_Orthographic	
*/
UT_Transform UT_MathHelper::MakeTransform_Orthographic(float znear, float zfar) 
{
	return MakeTransform_Scale(1.f, 1.f, 1.f / (zfar-znear)) *
		   MakeTransform_Translate(UT_Vec3Float(0.f, 0.f, -znear));
}
//-----------------------------------------------------------------------------------
UT_Transform UT_MathHelper::MakeTransform_Perspective(float fov, float n, float f) 
{
	// Perform projective divide
	UT_Matrix4x4Float persp = UT_Matrix4x4Float(1, 0,           0,              0,
												0, 1,           0,              0,
												0, 0, f / (f - n), -f*n / (f - n),
												0, 0,           1,              0);

	// Scale to canonical viewing volume
	float invTanAng = 1.f / tanf(Radians(fov) / 2.f);
	return MakeTransform_Scale(invTanAng, invTanAng, 1) * UT_Transform(persp);
}
//-----------------------------------------------------------------------------------
UT_BBOX_AABB UT_MathHelper::Union(const UT_BBOX_AABB &b, const UT_Point3Float &p) 
{
	UT_BBOX_AABB ret = b;
	ret.pMin.x = Min(b.pMin.x, p.x);
	ret.pMin.y = Min(b.pMin.y, p.y);
	ret.pMin.z = Min(b.pMin.z, p.z);
	ret.pMax.x = Max(b.pMax.x, p.x);
	ret.pMax.y = Max(b.pMax.y, p.y);
	ret.pMax.z = Max(b.pMax.z, p.z);
	return ret;
}
//-----------------------------------------------------------------------------------
UT_BBOX_AABB UT_MathHelper::Union(const UT_BBOX_AABB &b, const UT_BBOX_AABB &b2)
{
	UT_BBOX_AABB ret;
	ret.pMin.x = Min(b.pMin.x, b2.pMin.x);
	ret.pMin.y = Min(b.pMin.y, b2.pMin.y);
	ret.pMin.z = Min(b.pMin.z, b2.pMin.z);
	ret.pMax.x = Max(b.pMax.x, b2.pMax.x);
	ret.pMax.y = Max(b.pMax.y, b2.pMax.y);
	ret.pMax.z = Max(b.pMax.z, b2.pMax.z);
	return ret;
}
//-----------------------------------------------------------------------------------
UT_Vec3Float UT_MathHelper::ComputeSphericalDirection(float sintheta,float costheta, float phi)
{
	return UT_Vec3Float(sintheta * cosf(phi),
						sintheta * sinf(phi),
						costheta);
}
//-----------------------------------------------------------------------------------
UT_Vec3Float UT_MathHelper::ComputeSphericalDirection(float sintheta,float costheta, float phi, const UT_Vec3Float& x,const UT_Vec3Float& y,const UT_Vec3Float& z)
{
	return sintheta * cosf(phi) * x + sintheta * sinf(phi) * y + costheta * z;
}
//-----------------------------------------------------------------------------------
float UT_MathHelper::ComputeSphericalTheta(const UT_Vec3Float& v)
{
	return acosf(Clamp(v.z,-1.f,1.f));
}
//-----------------------------------------------------------------------------------
float UT_MathHelper::ComputeSphericalPhi(const UT_Vec3Float& v)
{
	float p = atan2f(v.y, v.x);
	return (p < 0.f) ? p + 2.f*UT_MathDefine::_PI : p;
}
//-----------------------------------------------------------------------------------
float UT_MathHelper::ComputeSphericalCosTheta(const UT_Vec3Float& v)
{
	return v.z;
}
//-----------------------------------------------------------------------------------
float UT_MathHelper::ComputeSphericalAbsCosTheta(const UT_Vec3Float& v)
{
	return fabsf(v.z);
}
//-----------------------------------------------------------------------------------
float UT_MathHelper::ComputeSphericalSinTheta2(const UT_Vec3Float& v)
{
	return Max(0.f,1.0f-v.z*v.z);
}
//-----------------------------------------------------------------------------------
float UT_MathHelper::ComputeSphericalSinTheta(const UT_Vec3Float& v)
{
	return sqrtf(ComputeSphericalSinTheta2(v));
}
//-----------------------------------------------------------------------------------
float UT_MathHelper::ComputeSphericalCosPhi(const UT_Vec3Float& v)
{
	float sintheta = ComputeSphericalSinTheta(v);
	if (sintheta == 0.f) return 1.f;
	return Clamp(v.x / sintheta, -1.f, 1.f);
}
//-----------------------------------------------------------------------------------
float UT_MathHelper::ComputeSphericalSinPhi(const UT_Vec3Float& v)
{
	float sintheta = ComputeSphericalSinTheta(v);
	if (sintheta == 0.f) return 0.f;
	return Clamp(v.y / sintheta, -1.f, 1.f);
}
//-----------------------------------------------------------------------------------
bool UT_MathHelper::IsSameHemiSphere(const UT_Vec3Float& w1,const UT_Vec3Float& w2)
{
	return w1.z * w2.z > 0.f;
}
//-----------------------------------------------------------------------------------
UT_Vec3Float UT_MathHelper::ReverseHemiSphere(const UT_Vec3Float& w)
{
	return UT_Vec3Float(w.x,w.y,-w.z);
}
//-----------------------------------------------------------------------------------
UT_SpectrumUsed UT_MathHelper::ComputeFresnelReflectionWithDielectirc(float fCosi, float fCost, const UT_SpectrumUsed& rkEtai,const UT_SpectrumUsed& rkEtat)
{
	UT_SpectrumUsed Rparl = ((rkEtat * fCosi) - (rkEtai * fCost)) / ((rkEtat * fCosi) + (rkEtai * fCost));
	UT_SpectrumUsed Rperp = ((rkEtai * fCosi) - (rkEtat * fCost)) / ((rkEtai * fCosi) + (rkEtat * fCost));
	return (Rparl*Rparl + Rperp*Rperp) / 2.f;
}
//-----------------------------------------------------------------------------------
UT_SpectrumUsed UT_MathHelper::ComputeFresnelReflectionWithConductor(float fCosi, const UT_SpectrumUsed& rkEta, const UT_SpectrumUsed& rkK)
{
	UT_SpectrumUsed tmp		= (rkEta*rkEta + rkK*rkK) * fCosi*fCosi;
	UT_SpectrumUsed Rparl2	= (tmp - (2.f * rkEta* fCosi) + 1) / (tmp + (2.f * rkEta * fCosi) + 1);
	UT_SpectrumUsed tmp_f	= rkEta*rkEta + rkK*rkK;
	UT_SpectrumUsed Rperp2	= (tmp_f - (2.f * rkEta * fCosi) + fCosi*fCosi) / (tmp_f + (2.f * rkEta * fCosi) + fCosi*fCosi);
	return (Rparl2 + Rperp2) / 2.f;
}